#include "SensorIntrinsic.h"
#include <fstream>


SensorIntrinsic::SensorIntrinsic ()
{
	_pMatrixCvMat = cvCreateMat (3, 3, CV_32FC1);
	_pDistCoeffsCvMat = cvCreateMat (5, 1, CV_32FC1);
}

SensorIntrinsic::~SensorIntrinsic ()
{
	cvReleaseMat (&_pMatrixCvMat);
	cvReleaseMat (&_pDistCoeffsCvMat);
}

float SensorIntrinsic::fx () const
{
	return _matrix[0][0];
}

float SensorIntrinsic::fy () const
{
	return _matrix[1][1];
}

float SensorIntrinsic::centerX () const
{
	return _matrix[2][0];
}

float SensorIntrinsic::centerY () const
{
	return _matrix[2][1];
}

float SensorIntrinsic::fw () const
{
	return _frameSize.x;
}

float SensorIntrinsic::fh () const
{
	return _frameSize.y;
}

float SensorIntrinsic::k1 () const
{
	return _distCeoffs[0];
}

float SensorIntrinsic::k2 () const
{
	return _distCeoffs[1];
}

float SensorIntrinsic::k3 () const
{
	return _distCeoffs[4];
}

float SensorIntrinsic::p1 () const
{
	return _distCeoffs[2];
}

float SensorIntrinsic::p2 () const
{
	return _distCeoffs[3];
}

const CvMat *SensorIntrinsic::matrixCvMat () const
{
	return _pMatrixCvMat;
}

const CvMat *SensorIntrinsic::distCoeffsCvMat () const
{
	return _pDistCoeffsCvMat;
}

void SensorIntrinsic::readFrom (const char *file)
{
	std::ifstream fIn (file);
	fIn >> _frameSize.x >> _frameSize.y;
	for (int c = 0; c < 3; ++c) {
		for (int r = 0; r < 3; ++r)
			fIn >> _matrix[c][r];
	}
	for (int i = 0; i < 5; ++i)
		fIn >> _distCeoffs[i];
	if (fIn.bad () || fIn.fail ())
		throw std::exception ("SensorIntrinsic::readFrom()"
				"Fail to read file");
	_updateCvMats ();
}

void SensorIntrinsic::writeTo (const char *file) const
{
	std::ofstream fOut (file);
	fOut << _frameSize.x << " " << _frameSize.y << std::endl;
	for (int c = 0; c < 3; ++c) {
		for (int r = 0; r < 3; ++r) {
			fOut << _matrix[c][r];
			if (c == 2) fOut << std::endl;
			else fOut << " ";
		}
	}
	for (int i = 0; i < 5; ++i)
		fOut << _distCeoffs[i] << " ";
	if (fOut.bad () || fOut.fail ())
		throw std::exception ("SensorIntrinsic::writeTo()"
				"Fail to write file");
}

const glm::mat3 & SensorIntrinsic::getMatrix () const
{
	return _matrix;
}

const float * SensorIntrinsic::getDistCeoffs () const
{
	return _distCeoffs;
}

void SensorIntrinsic::set (const glm::mat3 matrix, float distCeoffs[5])
{
	_matrix = matrix;
	memcpy (_distCeoffs, distCeoffs, sizeof (_distCeoffs));
}

glm::vec3 SensorIntrinsic::unproject (glm::vec3 p) const
{
	return glm::vec3 ((p.x - centerX ()) / fx () * p.z,
			(p.y - centerY ()) / fy () * p.z, p.z);
}

glm::vec2 SensorIntrinsic::project (glm::vec3 p) const
{
	glm::vec3 projected = _matrix * p;
	return glm::vec2 (projected.x / projected.z, projected.y / projected.z);
}

void SensorIntrinsic::_updateCvMats ()
{
	for (int r = 0; r < 3; ++r)
		for (int c = 0; c < 3; ++c)
			CV_MAT_ELEM (*_pMatrixCvMat, float, r, c) = _matrix[c][r];
	for (int r = 0; r < 5; ++r)
		CV_MAT_ELEM (*_pDistCoeffsCvMat, float, r, 0) = _distCeoffs[r];
}